#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include "usart.h"
#include "printf.h"
#include "user_task.h"
#include "debug.h"
#include "user_queue.h"
#include "tim.h"
#include "ctrl.h"
#include "ble.h"
#include "sbus.h"
osThreadId_t Task0Handle;
osThreadId_t Task1Handle;

const osThreadAttr_t Task0_attributes = { .name = "Task0",
		.stack_size = 512 * 4, .priority = (osPriority_t) osPriorityNormal, };
const osThreadAttr_t Task1_attributes = { .name = "Task1",
		.stack_size = 512 * 4, .priority = (osPriority_t) osPriorityNormal, };

void StartTask0(void *argument)
{
	USART2_RECEIVETYPE Usart2_rx;
	/* Infinite loop */
	for (;;) {
#ifdef DEBUG
//		debug_printf("task 0\r\n");
		while (0 != osMessageQueueGetCount(myQueue02Handle)) {
			osMessageQueueGet(myQueue02Handle, &Usart2_rx, 0U, 0U);
			ble_rx_deal(&Usart2_rx);
		}
#else
#endif
		portYIELD();
//		osDelay(2000);
	}
}
void StartTask1(void *argument)
{
	uint16_t ch[16] = { 0 };
	uint16_t pwm[16] = { 0 };
	uint8_t finish;
	uint8_t data[64];
	/* Infinite loop */
	for (;;) {
		sbus_Ceartframe(&finish, data);
		if (finish == HAL_OK) {
			sbus_Analysis(data, 0, ch);
			sbus_ch2pwm(ch, pwm);
			pwm_value((double) pwm[1], (double) pwm[3]);
//			do_run();
		}
		portYIELD();
//	  osDelay(1000);
	}
}
